#ifndef MS_OCTOMAP_OCTOMAP_GEN_H
#define MS_OCTOMAP_OCTOMAP_GEN_H

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl/point_types.h>
#include <pcl/ros/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/filters/passthrough.h>

#include <tf/transform_listener.h>
#include <tf/message_filter.h>
#include <message_filters/subscriber.h>
#if ROS_VERSION_MINIMUM(1,8,0) // test for Fuerte (new build system, separate stacks)
  #include <octomap_msgs/OctomapBinary.h>
#else
  #include <octomap_ros/OctomapBinary.h>
#endif

#include <visualization_msgs/MarkerArray.h>
#include <octomap_ros/conversions.h>
#include <octomap/octomap.h>
#include <octomap/OcTreeKey.h>

namespace octomap_gen {
class OctomapGen
{

public:
  typedef pcl::PointCloud<pcl::PointXYZ> PCLPointCloud;
  typedef octomap::OcTree OcTreeT;

  OctomapGen(ros::NodeHandle private_nh_ = ros::NodeHandle("~"));
  
  virtual ~OctomapGen();
  virtual void insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud);

protected:
  inline static void updateMinKey(const octomap::OcTreeKey& in, octomap::OcTreeKey& min){
    for (unsigned i=0; i<3; ++i)
      min[i] = std::min(in[i], min[i]);
  };
  
  inline static void updateMaxKey(const octomap::OcTreeKey& in, octomap::OcTreeKey& max){
    for (unsigned i=0; i<3; ++i)
      max[i] = std::max(in[i], max[i]);
  };
 
  void publishBinaryOctoMap(const ros::Time& rostime = ros::Time::now()) const;
  void publishFullOctoMap(const ros::Time& rostime = ros::Time::now()) const;

  /**
  * @brief update occupancy map with a scan labeled as ground and nonground.
  * The scans should be in the global map frame.
  *
  * @param sensorOrigin origin of the measurements for raycasting
  * @param pc all endpoints of the point cloud (clear up to occupied endpoint)
  */
  virtual void insertScan(const tf::Point& sensorOrigin, const PCLPointCloud& pc);

  ros::NodeHandle m_nh;
  ros::Publisher m_binaryMapPub, m_fullMapPub, m_markerPub;

  message_filters::Subscriber<sensor_msgs::PointCloud2>* m_pointCloudSub;
  tf::MessageFilter<sensor_msgs::PointCloud2>* m_tfPointCloudSub;
  ros::ServiceServer m_octomapBinaryService, m_octomapFullService, m_clearBBXService, m_resetService;
  tf::TransformListener m_tfListener;

  octomap::OcTree* m_octree;
  octomap::KeyRay m_keyRay;  // temp storage for ray casting
  octomap::OcTreeKey m_updateBBXMin;
  octomap::OcTreeKey m_updateBBXMax;

  double m_maxRange;
  std::string m_worldFrameId; // the map frame
  std::string m_baseFrameId; // base of the robot for ground plane filtering

  double m_res;
  unsigned m_treeDepth;
  unsigned m_maxTreeDepth;
  double m_probHit;
  double m_probMiss;
  double m_thresMin;
  double m_thresMax;

  double m_pointcloudMinX;
  double m_pointcloudMaxX;
  double m_pointcloudMinY;
  double m_pointcloudMaxY;
  double m_pointcloudMinZ;
  double m_pointcloudMaxZ;
  double m_minSizeX;
  double m_minSizeY;
  bool m_filterSpeckles;

};
}

#endif
